//
// Created by f on 2020/4/8.
//
#include "helper.h"

//该函数进行图像的畸变修正
void undist(cv::InputArray img, cv::OutputArray undt) {
    cv::Mat mtx = (cv::Mat_<double>(3,3)<<395.88158477404403, 0.0, 320.0517602702382,
                                                       0.0, 395.1860153721006, 228.60474070124627,
                                                       0.0, 0.0, 1.0);
    cv::Mat dist = (cv::Mat_<double>(1,5)<<-0.3637525157951758,
                                                        0.18003632145503493,
                                                        -0.0006812417994754639,
                                                        -0.0012075918601206523,
                                                        -0.05114153613296653);
    cv::undistort(img, undt, mtx, dist, mtx);
}

//筛选图像中的白色像素
void whiteThresh(cv::InputArray  img, cv::Mat& white, int white_threshold) {
    cv::Mat temp;
    cv::inRange(img,cv::Scalar(white_threshold, white_threshold, white_threshold),cv::Scalar(255,255,255), temp);
    vector<cv::Mat> temps(temp.channels());
    cv::split(temp,temps);
    white = temps[0];
}

//筛选图像中的黄色像素
void yellowThresh(cv::InputArray img, cv::Mat& img_yellow, int yellow, int thresh) {
    cv::Mat hsv;
    cv::cvtColor(img, hsv, cv::COLOR_BGR2HSV);
    cv::Mat temp;
    cv::inRange(hsv,cv::Scalar(yellow-thresh, 40, 100),cv::Scalar(yellow+thresh, 255, 255), temp);
    vector<cv::Mat> temps(temp.channels());
    cv::split(temp,temps);
    img_yellow = temps[0];
}

//透视变换
void perspectiveTransform(cv::InputArray img, cv::OutputArray warped, bool inv) {
    cv::Point2f src[4] = {cv::Point2f(B,H), cv::Point2f(IMG_COLS-B,H), cv::Point2f(0,IMG_ROWS), cv::Point2f(IMG_COLS,IMG_ROWS)};
    cv::Point2f dst[4] = {cv::Point2f(W,0), cv::Point2f(W+IMG_COLS,0), cv::Point2f(W, IMG_ROWS), cv::Point2f(W+IMG_COLS, IMG_ROWS)};
    cv::Mat M;
    if(inv){
        M = cv::getPerspectiveTransform(dst, src);
        cv::warpPerspective(img, warped, M, cv::Size(IMG_COLS, IMG_ROWS), cv::INTER_LINEAR);
    }
    else{
        M = cv::getPerspectiveTransform(src, dst);
        cv::warpPerspective(img, warped, M, cv::Size(IMG_COLS+2*W, IMG_ROWS), cv::INTER_LINEAR);
    }
}

//对图像进行canny边缘检测
void canny(cv::InputArray img, cv::OutputArray cannyed, int lower_threshold, int upper_threshold) {
    cv::Canny(img, cannyed, lower_threshold, upper_threshold);
}

//对图像进行霍夫变换，将符合条件的线描白
void hough(cv::InputArray cannyed, cv::InputOutputArray houghed, float slope_threshold) {
    vector<cv::Vec4i> lines;
    cv::HoughLinesP(cannyed, lines,1, CV_PI/180, 1,15, 5 );
    for(const auto& line:lines){
        float slope = static_cast<float>(line[3]-line[1]) / static_cast<float>(line[2]-line[0]);
        if(line[0]==line[2] || fabs(slope) >= slope_threshold){
            cv::line(houghed, cv::Point2i(line[0],line[1]), cv::Point2i(line[2],line[3]), cv::Scalar::all(255),5);
        }
    }
}

//该函数接收一张坐标平面转换后的二值化图片，在没有拟合多项式的情况下,使用滑动窗口找到车道线点，返回两侧车道线点的横纵坐标列表
void findPixels0(const cv::Mat& img, vector<cv::Point2i>& left_pts, vector<cv::Point2i>& right_pts){

    //计算图像下四分之一的纵向像素值之和histogram
    cv::Mat bottom_part = img(cv::Rect(0,img.rows/4*3,img.cols+0,img.rows/4));
    bottom_part.convertTo(bottom_part, CV_32F);
    vector<int> histogram(bottom_part.cols,0);
    for(int i=0; i<bottom_part.cols; ++i){
        for(int j=0; j<bottom_part.rows; ++j){
            histogram[i] += bottom_part.at<float>(j,i); //待优化
        }
    }

    int nwindows = 40;
    int window_height = img.rows/nwindows;
    //Set the width of the windows +/- margin
    int margin = 40;
    //Set minimum number of pixels found to recenter window
    int minpix = 100;


    int left_base = distance(histogram.begin(), std::max_element(histogram.begin()+W-100,histogram.begin()+histogram.size()/2));
    int right_base = distance(histogram.begin(), std::max_element(histogram.begin()+histogram.size()/2,histogram.end()-W+100));

    //vector<cv::Rect> left_windows;
    //vector<cv::Rect> right_windows;

    int left_current = left_base;
    int right_current = right_base;

    int win_xleft_low, win_xleft_high;
    int win_xright_low, win_xright_high;
    int win_y_low, win_y_high;

    float left_pts_x_sum;
    float left_window_pts_count;
    float right_pts_x_sum;
    float right_window_pts_count;


    for(int window=1;window<=nwindows;++window){
        win_xleft_low = std::max(left_current-margin,0);
        win_xleft_high = win_xleft_low + margin*2;
        win_xright_high = std::min(right_current+margin,img.cols);
        win_xright_low = win_xright_high-margin*2;
        win_y_high = img.rows-window*window_height;
        win_y_low = win_y_high + window_height;
        left_pts_x_sum = 0;
        left_window_pts_count = 0;
        right_pts_x_sum = 0;
        right_window_pts_count = 0;

        //std::cout<<left_current<<std::endl;
        //std::cout<<right_current<<std::endl;

        for(int row = win_y_high; row<win_y_low; ++row){
            const uchar* ptr = img.ptr(row);
            for(int col=win_xleft_low; col<win_xleft_high; ++col){
                //cout<<(int)ptr[col]<<endl;
                if((int)ptr[col] > 120){
                    left_pts_x_sum += col;
                    left_window_pts_count += 1;
                    cv::Point2i lp(col,row);
                    left_pts.push_back(lp);
                }
            }
        }

        for(int row = win_y_high; row<win_y_low; ++row){
            const uchar* ptr = img.ptr(row);
            for(int col=win_xright_low; col<win_xright_high; ++col){
                //cout<<(int)ptr[col]<<endl;
                if((int)ptr[col] > 120){
                    right_pts_x_sum += col;
                    right_window_pts_count += 1;
                    cv::Point2i rp(col,row);
                    right_pts.push_back(rp);
                }
            }
        }


        //更新左右侧窗口横向位置
        //划窗方向取决于已找到点的一次拟合方向
        if(left_window_pts_count>=minpix){
            left_current = left_pts_x_sum/left_window_pts_count;
        }
        else if (window>nwindows/3 && left_pts.size()>2){
            vector<float> left_lane_trend = fitLane(left_pts, 1);
            left_current = left_lane_trend[0] + left_lane_trend[1]*win_y_high;
        }
        //std::cout<<"left"<<left_current<<std::endl;

        if(right_window_pts_count>=minpix){
            right_current = right_pts_x_sum/right_window_pts_count;
        }
        else if (window>nwindows/3 && right_pts.size()>2){
            vector<float> right_lane_trend = fitLane(right_pts, 1);
            
            right_current = right_lane_trend[0] + right_lane_trend[1]*win_y_high;
        }
    }
}

//该函数接收一张坐标平面转换后的二值化图片，在已有拟合多项式的情况下,在多项式曲线附近收集像素，返回像素坐标
void findPixels1(const cv::Mat& img, vector<cv::Point2i>& left_pts, vector<cv::Point2i>& right_pts, const vector<float>& old_left_fit, const vector<float>& old_right_fit){
    int margin = 80;
    //这里使用遍历Mat Header中的指针+逐行遍历像素的方法，提升效率
    for(int row=0;row<img.rows;++row){
        const uchar* ptr = img.ptr(row);
        int l_line = old_left_fit[0]+old_left_fit[1]*row+old_left_fit[2]*row*row;
        int r_line = old_right_fit[0]+old_right_fit[1]*row+old_right_fit[2]*row*row;
        //左侧拟合线附近的遍历
        for(int col=l_line-margin; col<l_line+margin; ++col){
            if((int)ptr[col] != 0){
                cv::Point2i lpt(col,row);
                left_pts.push_back(lpt);
            }
        }
        //右侧拟合线附近的遍历
        for(int col=r_line-margin; col<r_line+margin; ++col){
            if((int)ptr[col] != 0){
                cv::Point2i rpt(col,row);
                right_pts.push_back(rpt);
            }
        }
    }
}

//用cv::solve求解方程系数。注意此处x是因变量，y是自变量。返回的方程系数从低到高排序
vector<float> fitLane(const vector<cv::Point>& pts, const int& order)
{
    //Number of key points
    int N = pts.size();

    //构造矩阵X
    cv::Mat X = cv::Mat::zeros(order + 1, order + 1, CV_64FC1);
    for (int i = 0; i < order + 1; i++)
    {
        for (int j = 0; j < order + 1; j++)
        {
            for (int k = 0; k < N; k++)
            {
                X.at<double>(i, j) = X.at<double>(i, j) + pow(pts[k].y, i + j);
            }
        }
    }

    //构造矩阵Y
    cv::Mat Y = cv::Mat::zeros(order + 1, 1, CV_64FC1);
    for (int i = 0; i < order + 1; i++)
    {
        for (int k = 0; k < N; k++)
        {
            Y.at<double>(i, 0) = Y.at<double>(i, 0) +
                                 std::pow(pts[k].y, i) * pts[k].x;
        }
    }

    cv::Mat A = cv::Mat::zeros(order + 1, 1, CV_64FC1);
    cv::solve(X, Y, A, cv::DECOMP_LU);

    vector<float> coefficient(order+1);
    for(int i=0; i<order+1; ++i) coefficient[i] = A.at<double>(i,0);
    return coefficient;
}

//该函数计算实际尺度下的车道线方程系数
vector<float> fit2real(const vector<float>& lane_fit){
    float ym_per_pix = 1.5/IMG_ROWS;
    float xm_per_pix = 0.4/IMG_COLS;
    vector<float> lane_fit_real(3);
    lane_fit_real[2] = lane_fit[2] / pow(ym_per_pix,2) * xm_per_pix;
    lane_fit_real[1] = lane_fit[1] /ym_per_pix * xm_per_pix;
    lane_fit_real[0] = lane_fit[0] * xm_per_pix;
    return lane_fit_real;
}

//该函数计算一侧车道线的曲率半径
float curverad(const vector<float> &lane_fit) {
    float y = 1.5;
    float cr;
    cr = pow((1 + pow((2*lane_fit[0]*y + lane_fit[1]),2)),1.5) / fabs(2*lane_fit[0]);
    return cr;
}

//该函数计算车辆中心与车道中心的距离。为正时车辆偏右
//输入的是真实尺度下的车道线方程系数
float offset(const vector<float> &left_fit, const vector<float> &right_fit) {
    double y = 1.5;
    float xm_per_pix = 0.4/IMG_COLS;
    float x_left = pow(left_fit[0]*y,2) + left_fit[1]*y + left_fit[2];
    float x_right = pow(right_fit[0]*y,2) + right_fit[1]*y + right_fit[2];
    float os = (x_left+x_right)/2 - (IMG_COLS/2 + W)*xm_per_pix;
    return os;
}

//该函数输入左右车道线方程参数、照片，在照片上绘制车道线
void drawLine(const cv::Mat &img, cv::Mat &lane_img, vector<float> &left_fit, vector<float> &right_fit){

    cv::Mat paded_zero = cv::Mat::zeros(IMG_ROWS,2*W+IMG_COLS,CV_8UC3);

    vector<cv::Point2i> pts;

    for(int y=0; y<IMG_ROWS; y++){
        cv::Point2i l_pt(left_fit[2]*pow(y,2) + left_fit[1]*y + left_fit[0],y);
        pts.push_back(l_pt);
    }

    for(int y=IMG_ROWS-1; y>=0; y--){
        cv::Point2i r_pt(right_fit[2]*pow(y,2) + right_fit[1]*y + right_fit[0],y);
        pts.push_back(r_pt);
    }

    //fillPoly函数的输入比较特殊，必须是1×n的二维向量
    vector<vector<cv::Point2i>> vpts;
    vpts.push_back(pts);

    cv::fillPoly(paded_zero, vpts, cv::Scalar(0,255,0));


    cv::Mat lane_area;
    perspectiveTransform(paded_zero, lane_area, true);

    cv::addWeighted(img,1,lane_area,0.3,0.,lane_img);


    vector<float> left_fit_real = fit2real(left_fit);
    vector<float> right_fit_real = fit2real(right_fit);
    float left_curverad = curverad(left_fit_real);
    float right_curverad = curverad(right_fit_real);
    float ofst = offset(left_fit_real, right_fit_real);
    string text1 = "Left curvature radius: " + std::to_string(left_curverad);
    string text2 = "Left curvature radius: " + std::to_string(right_curverad);
    string text3;
    if (ofst > 0) { text3 = std::to_string(ofst) + " right of center"; }
    else { text3 = std::to_string(ofst) + " left of center"; }

    cv::putText(lane_img, text1, cv::Point2i(10, 20), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(100,100,200), 1.5);
    cv::putText(lane_img, text2, cv::Point2i(10, 40), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(100,100,200), 1.5);
    cv::putText(lane_img, text3, cv::Point2i(10, 60), cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(100,100,200), 1.5);
}